#include "jsoncpp/json/json.h"
#include <mosquitto.h>
#include <iostream>
#include <functional>
using Callback = std::function<void(const std::string,const std::string)>;
class Mqtt_client
{
    public:
        Mqtt_client()
        {
            mqtt_count=0;     
               
        }
        Mqtt_client(std::string sub,std::string pub)
        {
            sub_topic=sub;
            pub_topic=pub;  
               
        }
       
        void setCallback(Callback func) {
            m_callback = func;
        }
        void connect()  
        {
            // 初始化 Mosquitto 库
            mosquitto_lib_init();
            // 创建 MQTT 客户端实例
            mqtt_client = mosquitto_new(nullptr, true, this);
            if (!mqtt_client)
            {
              //  RCLCPP_ERROR(this->get_logger(), "Failed to create MQTT client");
                return;
            }
            // 设置回调函数
            mosquitto_connect_callback_set(mqtt_client, on_connect);
            mosquitto_message_callback_set(mqtt_client, on_message);
    
            // 连接到 MQTT Broker
            if (mosquitto_connect(mqtt_client, "localhost", 1883, 60) != MOSQ_ERR_SUCCESS)
            {
               // RCLCPP_ERROR(this->get_logger(), "Failed to connect to MQTT Broker");
                return;
            }
    
            // 订阅主题
            mosquitto_subscribe(mqtt_client, nullptr, sub_topic.c_str(), 0);
    
            // 启动循环以处理MQTT消息
            mosquitto_loop_start(mqtt_client);
    
        } 
        ~Mqtt_client()
        {
            mosquitto_destroy(mqtt_client);
            mosquitto_lib_cleanup();
        }
        void publish_message()
        {
            // int value = 42; // 示例整型数据
            mqtt_count++;
            Json::Value json_val;
            json_val["ros_count"] = mqtt_count;
            json_val["x"] = 0.1f;
            json_val["y"] = 0.2f;
            publish(pub_topic, json_val);
        }  
        void publish(const std::string &msg)
        {
            publish(pub_topic,msg);
        }
        void publish(const std::string &topic, const std::string &msg)
        {
            if (mosquitto_publish(mqtt_client, nullptr, topic.c_str(), msg.size(), static_cast<const void *>(msg.c_str()), 0, false) != MOSQ_ERR_SUCCESS)
            {
               // RCLCPP_ERROR(this->get_logger(), "Failed to publish message");
               std::cout<<"Failed to publish message"<<std::endl;
            }
            else
            {
                // RCLCPP_INFO(this->get_logger(), "Message published: %d", value);
               // std::cout<<"Message published: "<<value<<std::endl;
            }
        }
        void publish(const std::string &topic, const Json::Value& json_val)
        {
            publish(topic, json_val.toStyledString());
        }
        
        static void on_connect(struct mosquitto *client, void *userdata, int rc)
        {
            auto node = static_cast<Mqtt_client *>(userdata);
            if (rc == 0)
            {
            //  RCLCPP_INFO(node->get_logger(), "Connected to MQTT Broker");
             std::cout<<"Connected to MQTT Broker"<<std::endl;
            }
            else
            {
            //  RCLCPP_ERROR(node->get_logger(), "Failed to connect to MQTT Broker");
            std::cout<<"Failed to connect to MQTT Broker"<<std::endl;
            }
        }
        static void on_message(struct mosquitto *client, void *userdata, const struct mosquitto_message *message)
        {
            auto node = static_cast<Mqtt_client *>(userdata);
            if (node)
            {
                node->onMessage(std::string(message->topic), std::string((char *)message->payload, message->payloadlen));
            //    if (m_callback) {
            //         m_callback(std::string(message->topic), std::string((char *)message->payload, message->payloadlen)); // 触发回调 
            //     }
            }
        }

        void onMessage(const std::string &topic, const std::string &msg)
        {
            // RCLCPP_INFO(get_logger(), "recv topic: %s, msg: %s", topic.c_str(), msg.c_str());
            // try
            // {
            //     Json::Reader reader;
            //     Json::Value json_val;
            //     if(reader.parse(msg, json_val))
            //     {
                    
            //        // RCLCPP_INFO(get_logger(), "recv topic: %s, msg: %s", topic.c_str(), json_val.toStyledString().c_str());
            //         int cmd = json_val["cmd"].asInt();
            //         if(cmd == 0)
            //         {
            //             double x = json_val["pose"]["x"].asDouble();
            //             double y = json_val["pose"]["y"].asDouble();
            //             double yaw = json_val["pose"]["yaw"].asDouble();
            //           //  RCLCPP_INFO(get_logger(), "pose: %.3f, %.3f, %.3f", x, y, yaw);
            //         }
            //     }
            // }
            // catch(const std::exception& e)
            // {
            //     std::cerr << e.what() << '\n';
            // }
           
                         m_callback(topic,msg); // 触发回调 
                  
        }
        struct mosquitto *mqtt_client;
    private:
       
    int mqtt_count; 
    std::string sub_topic,pub_topic;
    Callback m_callback = nullptr;
    void* m_context = nullptr;
};